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Abstract 


The center of mass space is a convenient space for planning motions that minimize reaction forces at the 
robot’s base or optimize the stability of a mechanism. A unique problem associated with path planning in the center 
of mass space is the potential existence of multiple center of mass images for a single Cartesian obstacle, since a 
single center of mass location can correspond to multiple robot joint configurations. The existence of multiple 
images results in a need to either maintain multiple center of mass obstacle maps or to update obstacle locations 
when the robot passes through a singularity, such as when it moves from an elbow-up to an elbow-down 
configuration. To illustrate the concepts presented in this paper, a path is planned for an example task requiring 
motion through multiple center of mass space maps. The object of the path planning algorithm is to locate the bang- 
bang acceleration profile that minimizes the robot's base reactions in the presence of a single Cartesian obstacle. To 
simplify the presentation, only non-redundant robots are considered and joint non-linearities are neglected. 


1. Introduction 

The center of mass (CM) space is the space 
reachable by a robot’s CM as the robot moves around 
its workspace. The CM space is important because 
motion of the robot's CM in the CM space can be 
directly related to reaction forces at the robot’s base . 1 * 
In addition, location of a mechanism’s CM relative to 
its support points is used to control mechanical 
stability . 2,3 Many of the planning techniques applicable 
to motion in Cartesian space are directly applicable to 
planning in the CM space. However, the CM space has 
a unique characteristic related to obstacle locations. A 
single obstacle in Cartesian space can map to several 
CM space images, since a single center of mass location 
can correspond to multiple robot joint configurations. 
This issue will be explored in depth in this paper. 
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Traditionally motion planning has been 
accomplished in either Cartesian space or robot joint 
space . 4 The CM space is a Cartesian like space 
allowing direct application of many of the existing 
control techniques. Boulic et. al. demonstrated a 
resolved rate control scheme applied to the motion of 
redundant serial kinematic chains containing sixteen or 
more degrees of freedom . 5 They also introduced a 
method for outlining the CM space. Papadopoulos et 
al. described a technique for designing a robot with zero 
base reactions in a limited region of its reachable space, 
termed the reactionless workspace . 6 The design was 
based on an analysis of the motion of the robot's CM. 
Neither of the above authors investigated path planning. 
In addition, neither author addressed motion within the 
CM space in the presence of obstacles. In Doggett et 
al. equations describing the path which globally 
minimizes the robot’s base reaction forces while 
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Path planning in the presence of obstacles 
naturally decomposes into planning paths between 
intermediate goals around the obstacles. Thus a series 
of segments are planned, which when combined, yield 
the complete path from the initial configuration to the 
final configuration. To locate the best path a minimax 
optimization approach will be used. This is a technique 
first receiving wide spread use in the signal processing 
community. The technique seeks to minimize the 
maximum of a group of functions, in this case the peak 
reaction forces. Nowrouzian et al. applied minimax 
optimization to simultaneously meet magnitude and 
group delay specifications in digital filter design. 7 
Crow et. al. used minimax techniques to locate 
statistical discontinuities for applications such as traffic 
monitoring and mechanical failure detection. 8 

This paper will follow a representative 
planning scenario to illustrate an issue unique to CM 
space path planning related to the potential existence of 
multiple CM space images of a single Cartesian 
obstacle. The objective of the planning scenario will be 
to plan paths minimizing the base reaction force while 
executing a bang-bang acceleration profile. First, the 
equations will be given for the bang-bang profiles along 
with a description of the optimization approach used to 
locate the bang-bang path minimizing the robot's base 
reaction forces. Second, the issue of mapping the 
obstacles from Cartesian space to equivalent CM space 
images will be addressed. Third, a path will be planned 
for a task that requires the robot to pass through a 
singularity. 

The approach presented assumes the initial 
positions of the robot joints are given and, during the 
maneuver, the mass of the robot and the obstacle 
position remain constant. This paper does not address 
the issue of smoothing the acceleration profiles between 
successive paths in an optimal manner. Much work has 
already been done in this area ( see, for example, the 
work by Bell. 9 ]). Further, the paper does not address 
the problem of finding all possible paths from the initial 
robot configuration to the final CM location. Several 
techniques have been suggested for developing this 
information, see Latombe. 10 


2. Development 

In this section the equations describing the 
path that the CM will follow are developed. The 
equations are formulated assuming that the CM 
acceleration profile is bang-bang. The objective is to 
select the path that minimizes the peak acceleration of 
the CM during the move. While this is not the global 
minimum, due to the bang-bang assumption, it could be 
used as an estimate for further calculations to locate the 
global minimum. 


To formulate the equations, consider motion 
from A to C over an obstacle with peak at B as 
illustrated in Fig. 1. The motion comprising a 
horizontal distance d over an obstacle with height K b s 
located at X obs must be completed in T seconds. The 
velocity and position at A and C are assumed known. 

Le - v *a ’ v *c ’ v y c ’ x c * and Jc ^ 
where 


x A 9 x B 9 x c 


V ,v_ ,v, 

X A X B X C 


are the locations along the x axis at A, 
or C respectively, 

are the locations along the y axis at A, 

B , or C respectively, 

are the velocities along the x axis at 


A, B, or C respectively, and 
V , v , V are the velocities along the y axis at 
A, B , or C respectively. 

Figure 2 depicts a representative pair of bang-bang 
acceleration profiles. As can be seen from the figure, a 
single switch from acceleration to deceleration along jt 
can occur. The switch occurs T sx seconds from the 
initial point at A. Two potential switches points exists 
for the y acceleration occurring at T x or T sy2 . The 


objective is to minimize the Euclidean norm of the 


acceleration, i.e. I a x 



, while simultaneously 


satisfying the boundary conditions at A and C. The first 
step is to locate the switch times by integrating the 
acceleration profile twice and applying the boundary 
conditions. The x boundary conditions will be satisfied 
in the allotted time T by one of two distinct cases. 

Case 1) It is possible to move from A to C with a 
constant acceleration, which may be zero, 
or 

Case 2) an interval of acceleration and deceleration 
are necessary to move from A to C in the 
allotted time. 

When a is constant, integrating twice leads to the 
following expression for position 

x{t) — \at 2 + v o t + X Q , where V Q and X 0 are the 


velocity and position at t — 0 , respectively. 

The first case implies 

x ac (0 = T«/+V + * /4 ,fe[0,7l} 

v *Jt) = aJ + v XA ,{te[0,T]} 

which leads to 


d = j a x T +v x T+x a 

v r - aT + v r ' 

x c x x A 


Case 1 holds if and only if 


( 1 ) 

( 2 ) 
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In general, the second case will occur and 
additional calculations are necessary to determine the 
acceleration and switch time. Let t x be the time relative 
to the start of the first motion interval, AB ; let t 2 be 
the time relative to the start of the second interval, BC ; 
and let T sx be the time from the start of the first interval 

y acceleration 

x acceleration 


acceleration[nVsec A 2] 



Figure 2, Two dimensional Bang-Bang x and y Acceleration Profile. 
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until the switch from acceleration to deceleration 
occurs. Referring to Fig. 1 

X A B (tx,)=Tajl +V*, +*4>K e [°>^]} ( 4 ) 

X Bc( t x 1 ) = ~T a x t x 1 + V xJx 2 + X B 


>K e[0,r-7; x ]} (5) 

x bc{T~ T sx ) — d ( 6 ) 

( r „ ) = a A, + , {t X] e [0, T sx ]} (7) 

v(0 =_a ^ +vK G [°’ r - r -]}( 8 ) 

v Xbc {T-T sx ) = v Xc (9) 

Combining Eqs. (4), (5), (6), and (7) leads to 

d = ?a x T 2 -a x (T-T sx ) + v x T + x A . (10) 

Combining Eqs. (7), (8), and (9) leads to 
V x c =- a A T - T s*) + a x T s X +V * A - (") 


Solving Eq. (1 1) for d x and substituting for a x in Eq. 
(10) leads to 

{T - J 2 [-2(v, c - V, J] + (r - T a )[4(rf -v x T- x A )] 
+ [( v x c + y 2 + 2t { x a -^)]=° 

which can be solved for (T - rj via the quadratic 
formula. With T x known, a x can be calculated using 
Eq. (10). 

Now that the motion in x has been calculated, 
the equations describing the y motion can be 
formulated. The time to reach the obstacle, T obs , is set 
by the motion in the x direction and can be calculated as 
follows. If the obstacle is encountered during the first 
acceleration interval, i.e. , then T obs can be 

calculated using the known value of a x from 

x obs = T aT obs + v Xa T obs + X A . If, however, T rM > T a , 
then using Eqs. (4), (5), (6), and (7) leads to 

X obs —-~2 a x {fobs ~ T sx ) 

+ { a x T sx +V x A ){ T obs~ T sx) + \ a x T s X +V xJsx +X A 

whch can be solved for T obs via the quadratic formula. 

With T obs known, the derivation described 
above for the x motion is repeated for each y interval, 
AB of duration T 0 bs and BC of duration T-T obs , to find 
T vl and T^of Fig. 2. However, the y velocity at the 

obstacle top, , is unknown. This velocity is 
determined by minimizing the peak acceleration, a y . 
The minimization is performed using a minimax 
optimization strategy with two state variables. The two 
states correspond to the y velocity at B and the height 
above the obstacle. The height above the obstacle is a 
state variable, because it is possible for the initial and 
final y velocities to be such that moving well above the 
obstacle is more efficient than passing just above the 



obstacle. A key constraint in the optimization is that 
each interval must have the same minimum peak 


acceleration. To illustrate why, consider a case where 
the first interval's minimum peak acceleration was less 
than that of the second interval. Then the magnitude of 
first acceleration profile could be increased, providing 
more time for the second interval to complete, thus 
reducing its peak acceleration. This trade off would 
continue until the optimal motion is determined, where 
each interval has the same minimum peak acceleration, 


3. Representative Planning Scenario 

In this section a representative planning 
scenario will be used to illustrate a unique issue related 
to CM space path planning. The scenario is based on 
the two degree of freedom (DOF) robot with revolute 
joints shown in Fig. 3. The quantities , l 2 , / cl , l c2 , m { 
and m 2 are the link length, location of the link CM 
along the link, and link mass for joints 1 and 2, 


3t 



Figure 4. Initial Robot Configuration and Goal 
Requiring Motion through Singularity (i.e. to CM 
Space Boundary). 


4 

American Institute of Aeronautics and Astronautics 






the right. The objective is to plan a path from the robot 
configuration shown to a configuration where the 
robot's end point is just touching the box at the top 
center identified by "goal". The first step in CM space 
path planning is to map the obstacles into the CM space 
by sliding the robot around the obstacle and noting the 
location of the robot's CM as shown in Fig. 5. Figure 5 
was constructed based on the following values: 


hml 2 =4\m\ (12) 

m x =m 2 =\[kg] (13) 

T = 1 [sec] (15) 


In Fig. 5 the two circles correspond to the inner and 
outer CM boundaries and the dots represent the robot 
CM location for a given arm configuration. The 
reachable CM space is the area between the two 
concentric circles. The inner circle is formed by 
folding the robot over on itself, such that G 2 = iso , and 
moving the first joint through one complete revolution, 
0^0° -4 180° . The outer circle is formed in a similar 
fashion with the robot fully extended, d 2 = 0°. For the 
purpose of this example joint range limits have been 
ignored. However, if range limits existed, this would 
have no affect on the algorithm presented. Range limits 
only affect the shape of CM space. 

Notice that the robot is maintaining the elbow- 
up configuration during the formation of the CM image 
of Fig. 5. (Elbow-up is defined as the robot 
configuration corresponding to location of joint 2 on the 
left side of a vector originating at joint 1 and passing 


through the robot end-point as viewed looking along the 
vector from a point above, positive Z in the figure, the 
origin.) Alternatively, the robot could maintain an 
elbow-down configuration. This leads to two potential 
CM space images for the table of Fig. 4 corresponding 
to the two robot parities as shown in Fig. 6. If 
additional obstacles were present in the robot's 
Cartesian workspace, each obstacle could be mapped 
individually, then composite CM space maps would be 
formed from a union of the elbow-up and elbow-down 
images, respectively. This approach would enable a 
planner to add more detail as necessary. In this case, 
only collisions with the table are considered, however it 
is also important not to pass through the box while 
moving to the goal. It will be shown that collision 
checks with box are unnecessary because the optimum 
path for this problem approaches the box from above, 
completely avoiding contact with the box. 

The planning problem can be viewed in the 
CM space as moving from the darkened circle labeled A 
at the bottom of Fig. 7 to the square labeled E. In Fig. 7 
the small circles represent the initial configuration of 
the robot, while the small squares identify the potential 
final configurations. The darkened circle and square 
identify the elbow-down configurations for the initial 
and final configuration, respectively. Notice that the 
final elbow-down configuration is completely enclosed 
in the elbow-down obstacle image. This indicates that a 
collision with the obstacle would occur before the robot 
was able to reach the box in the elbow-down 
configuration. Thus, the robot must approach the top of 
the box in an elbow-up configuration. 
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Figure 6. Multiple CM Space Images of Single Cartesian Obstacle. 


The next question is how can the information 
about the two CM images of the obstacle be used. One 
idea is to recombine the two images into a single image 
either by a union of each image in the CM space or by 
enclosing both images within a single object. It will be 


shown in a representative scenario that either approach 
is unacceptable, because each artificially restricts the 
reachable CM space. Instead, in the case of the 2 DOF 
robot, two CM space maps must be maintained. The 
two maps can be viewed as two surfaces of a planar 



Figure 7. Two Possible CM Space Paths to Final Configuration 
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annulus. § One, the top, represents the elbow-up robot 
map or elbow-up robot parity , the other the elbow- 
down parity. Transitions between the two maps can 
only occur at the radial boundaries of the annulus. 
Figure 6 is a view from above the transparent annulus 
allowing simultaneous viewing of both obstacles. 

There are two potential strategies for moving 
from the initial elbow-down configuration to the final 
elbow-up configuration. Each requires motion through 
a singularity either at the inner or outer CM space 
boundary.** The first strategy is to fold the robot 
completely over on its self (0 2 = 180°)> moving the 
robot's CM to the inner CM space boundary. The 
second strategy is to fully extend the robot (, 0 2 = 0°), 
moving the robot's CM to the outer CM space 
boundary. This second strategy would be completely 
overlooked if only a single CM map was maintained. 

The following scheme is used to locate the 
optimum bang-bang path to the inner boundary. 
Referring to the dashed line in Fig. 7, the objective is to 
go from A to some point on the inner CM boundary, 
denoted by C, and then over the elbow-up image peak, 
indicated by D, to E , The optimization is formulated as 
a minimax problem, where the objective is to minimize 
the maximum acceleration along each sub-path, A C and 
CE. Each sub-path is located using the method 
described in Sec. 2. The boundary conditions for the 
overall move are that the initial and final velocities are 
zero, while the initial position corresponds to A and the 
final position corresponds to E. At position C, 
continuity in position and velocity are imposed. 

To reduce the dimension of the state space 
searched by the optimization procedure, several 
characteristics of the problem geometry can be 
exploited. First, the inner boundary is a circle of radius 
2, and thus the;; coordinate of C uniquely identifies C s 
location in the reasonable search region. Second, the 
singularity condition at the inner boundary forces the 
velocity at C to be tangent to the inner circle. Therefore 
the optimization attempts to minimize the maximum 
acceleration by determining the optimal values of the 
following state variables: (i) the time from A to C, (ii) 
the y coordinate of C, and (iii) the magnitude of the 
tangent velocity at C 

The resulting trajectory is shown in Fig. 8 by 
the dashed line. The elbow-up section is indicated by 


A washer, as in a bolt, nut, and washer. 

A singular configuration is a robot 
configuration that reduces the useable degrees of 
freedom of the robot. For example, with the robot fully 
extended, e 2 = o°, end point motion is only possible 
perpendicular to the axis of the final link, l 2 . Thus the 
2 DOF robot of Fig. 3, has only 1 DOF, in this 
configuration, i.e. motion along the tangent line. 


open triangles while darkened inverted triangles are 
used to indicate the elbow-down section of the path. 
This path results in a peak acceleration of 26.8 m/s 2 . 
The final values for the three state variables indicated 
that the time to move from A to C was 0.45 seconds 
while the time to move from C to E was 0.55 seconds. 
The tangent velocity at C was -6.3 m/s 2 and the path 
intersects the inner circle at ay coordinate of 0.1m. 

While joint limits have been ignored in this 
example, the joint positions, velocities, and 
accelerations must be realistic, especially as the robot 
moves through the singularity. Figures 9 and 10, are 
plots of the joint positions and velocities, respectively. 
The joint positions appear smooth and the joint 
velocities are finite at the singularity. The step change 
in Fig. 9 at approximately 5.5 seconds is due to a 
wrapping from 2n to -2k. The sharp transitions in the 
joint velocities of Fig. 10 are expected from the bang- 
bang acceleration profiles. Optimal smoothing of these 
transitions was not addressed in this paper, however 
other researchers have suggested potential schemes. 9 

The second possible strategy is to move from 
A of Fig. 7 over the elbow-down image at B to the outer 
CM boundary, indicated by C, and then to E avoiding 
the elbow-up image. As in the first strategy, we are 
able to take advantage of the geometry to reduce the 
state space to the same three variables used above. The 
trajectory resulting from the minimax optimization is 
shown in Fig. 1 1 . Notice that the path appears to move 
through the elbow-up image, however this portion of 
the path is executed on the bottom of the annulus where 
only the elbow-down image is present. The elbow-up 
image becomes important when the CM reaches the 
outer CM space boundary, and the planner begins to use 
the elbow-up CM map, avoiding the elbow-up image, 
as the robot moves to E. The final values for the three 
state variables indicated that the time to move from A to 
C was 0.7 seconds while the time to move from C to E 
was 0.3 seconds. The tangent velocity at C was 3.0 
m/s 2 , and the path intersects the inner circle at a y 
coordinate of 0.68m. This path results in a uniform 
peak force of 23.2 N at the base of the 1kg robot, which 
is considerably better than the 26.8 N of the path to the 
inner boundary. It is important to remember that if two 
separate obstacle maps had not been maintained, then 
this path would not have been located. While this is not 
catastrophic in this situation, it could be if the path to 
the inner boundary was blocked by another obstacle. If 
the inner boundary path is blocked and only a single 
CM space map is maintained, then the task would be 
erroneously labeled infeasible. 
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Figure 8. Optimum Bang-Bang Path to Inner Boundary 



Figure 9. Joint Positions for Motion to Inner Boundary 
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Figure 10. Joint Velocities for Motion to Inner Boundary 



Figure 11. Optimum Bang-Bang Path to Outer Boundary 
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Figure 12. CM Acceleration for Optimum Bang-Bang Path to Outer Boundary 


The CM acceleration profile for the path of 
Fig. 1 1 is shown in Fig. 12. Notice that the Euclidean 
norm of the (x,y) acceleration is a constant 23.2 m/s 2 as 
expected from the bang-bang assumption. Note that the 
x andy acceleration profiles do not appear to be moving 
(banging) between two constant extreme acceleration 
values. This is because the bang-bang acceleration 
profiles are calculated relative to the line segments AC 
and CE of Fig. 11. The accelerations of Fig. 12 are 
relative to the x andy axes of Fig. 1 1 . 

While no attempt was made to optimize the 
joint torques, a comparison shows that the path to the 
outer boundary is slightly better than the path to the 
inner boundary. The joint torques required to execute 
the paths can be calculated after the known CM 



Figure 13. Joint 1 Torque for paths to Inner and Outer 
CM space boundaries 


accelerations are used to 
calculate the joint 
accelerations. As shown in 
Fig. 13, the peak torque 
occurs in joint 1 and is 
approximately equal for 
both motions. The high 
torque at the end of the 
motion to the outer 
boundary is due to the 
necessity of maneuvering 
with the robot nearly fully 
extended when its 
rotational inertia is 
maximal. Figure 14 shows 
the superiority of the 
motion to the outer 
boundary in terms of peak 
joint 2 torque. The motion 
to the inner boundary 
requires joint 2 to move 
through more than 270 
degrees compared to less 
than 90 degrees for the 
motion to the outer boundary, resulting in the higher 
joint torque profile shown in Fig. 14. 

Figures 15 and 16 display the robot motions 
corresponding to the paths to the inner and outer CM 
space boundaries shown in Figs. 8 and 1 1 , respectively. 
In Figs. 15 and 16, the obstacle is shown in black while 
a solid line indicates the motion of the robot tip. The 
motion to the outer boundary appears to be more 
efficient as expected from the comparison of the peak 
acceleration along each path. 

Conclusions 

In this paper the effect of singularities on the 
planning process in the center of mass (CM) space has 



Figure 14. Joint 2 Torque for paths to Inner and Outer 
CM space boundaries 
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been investigated. The CM space is a Cartesian- like 
space useful for planning paths that minimize robot 
base reaction forces or maintain the stability of a 
mechanism. This paper has introduced a problem 
unique to CM space path planning. The problem stems 
from the existence of multiple CM space images of a 
single Cartesian obstacle. As shown in a representative 
planning scenario, existence of multiple images 
necessitates maintenance of multiple CM space maps, 
each map specific to a particular robot parity , for 
example elbow-up vs. elbow-down. To begin the 
planning process a unique map is selected based on 
initial robot parity. If singularities exist in the 
workspace , then path to/through the singularities must 
be planned and alternate CM maps explored. The 
minimal path is then determined by selecting the 
optimal path from the set of all complete paths found. 

This process was examined in detail through a 
planning scenario based on a two-degree-of-freedom 
robot avoiding an obstacle. The objective was to locate 
the bang-bang path that resulted in the minimum base 
reaction force while accomplishing a task. It was 
shown that it was necessary to maintain multiple CM 
space maps for effective planning, each specific to a 
particular robot parity. Failure to maintain separate CM 
space maps artificially reduced the number of potential 


strategies that could be applied to solve the specified 
task. At best this would reduce the efficiency with 
which the task was accomplished, as illustrated by the 
representative planning scenario. At worst the task 
could be erroneously labeled unfeasible. 
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